#include "Math/LB_Math.h"
#include <Algorithm/threeD_elementary_geometry_tool_library.h>
// #include<cstring>


// 这里应该抽象出来点到n维单形最近点的算法。
// back_dim 应该大于等于len-1
// len>=1
// @ return 1：这个最近点就是单形所在线性空间的全局最近点
// 0:这个最近点不是单形所在线性空间的全局最近点
// -1: 计算失败
int compute_neareat_point2simplex(double** ps,int back_dim,int len,double*p,double* lambda )
{
    // int re=1;
    if(len<1||back_dim<len-1){return -1;}
    if(len==1)
    {
        lambda[0]=1;
        return 1;
    }
    else if(len==2)
    {
        double fac1=0,fac2=0;
        for(int i=0;i<back_dim;i++)
        {
            fac1+=(ps[1][i]-ps[0][i])*(ps[1][i]-ps[0][i]);
            fac2+=(ps[1][i]-ps[0][i])*(p[i]-ps[0][i]);
        }
        lambda[1]=fac2/fac1;
        int re=0;
        if(lambda[1]>1)
        {
            lambda[1]=1;
        }
        else if (lambda[1]<0) {
            lambda[1]=0;
        }
        else{re=1;}
        lambda[0]=1-lambda[1];
        return re;
    }
    memset(lambda, 0, sizeof(double)*len);

    LB_Matrix mat;
    lb_matrix_init_double(&mat,len-1,len-1);
    for(int i=0;i<len-1;i++)
    {
        for(int j=i;j<len-1;j++)
        {
            *((double*)(mat.element(&mat,i,j)))=0;
            for(int k=0;k<back_dim;k++)
            {
                *((double*)(mat.element(&mat,i,j)))+=(ps[i+1][k]-ps[0][k])*(ps[j+1][k]-ps[0][k]);
            }
            *((double*)(mat.element(&mat,j,i)))=*((double*)(mat.element(&mat,i,j)));

        }
    }
    LB_Matrix* mati=mat.inverse(&mat);
    free(mat.data);

    if(mati==NULL){return -1;}
    double* y=(double*)malloc(sizeof(double)*(len-1));
    // memset(y, 0, sizeof(double)*(len-1));
    for(int i=0;i<len-1;i++ )
    {
        y[i]=0;
        for(int k=0;k<back_dim;k++)
        {
            y[i]+=(p[k]-ps[0][k])*(ps[i+1][k]-ps[0][k]);
        }
    }
    double* data=(double*)(mati->data);
    lambda[0]=0;
    for(int i=0;i<len-1;i++)
    {
        lambda[i+1]=0;
        for(int k=0;k<len-1;k++)
        {
            lambda[i+1]+=data[i*(len-1)+k]*y[k];
        }
        lambda[0]+=lambda[i+1];
    }
    lambda[0]=1-lambda[0];
    free(y);

    lb_matrix_free(mati);
    int flag=1;
    for(int i=0;i<len;i++)
    {
        if(lambda[i]<0){flag=0;break;}
    }
    if(flag==1){return 1;}

    double* lambda1=(double*)malloc(sizeof(double)*(len-1));
    double** ps1=(double**)malloc(sizeof(double*)*(len-1));
    double dis=-1;
    for(int i=0;i<len;i++)
    {
        memset(lambda1, 0,sizeof(double)*(len-1) );
        int k=0;
        for(int j=0;j<len;j++)
        {
            if(j!=i)
            {
                ps1[k]=ps[j];
                k++;
            }
        }

        int temp_re=compute_neareat_point2simplex(ps1,
            back_dim,len-1,p,lambda1 );
        if(temp_re==-1){continue;}
        double temp_dis=0;
        for(int j=0;j<back_dim;j++)
        {
            double tempd=p[j];
            for(k=0;k<len-1;k++)
            {
                tempd=tempd-lambda1[k]*ps1[k][j];
            }
            temp_dis+=tempd*tempd;
        }

        if(temp_re==1||dis<0||temp_dis<dis)
        {
            lambda[i]=0;
            dis=temp_dis;
            k=0;
            for(int j=0;j<len;j++)
            {
                if(j!=i)
                {
                    lambda[j]=lambda1[k];
                    k++;
                }
            }
        }
        if(temp_re==1)
        {
            break;
        }
    }
    free(ps1);
    free(lambda1);

    return 0;
}
//@ return array which have four length

void compute_plane_normal_of_jordan_curve(double** v,int len,double* re )
{
    // double* re=(double*)malloc(sizeof(double)*3);
    memset(re,0,sizeof(double)*3);
    double n[3]={0};
    for(int i=0;i<len;i++)
    {
        int j=(i-1+len)%len,k=(i+1)%len;
        double a[3]={v[i][0]-v[j][0],v[i][1]-v[j][1],v[i][2]-v[j][2]};
        double b[3]={v[k][0]-v[i][0],v[k][1]-v[i][1],v[k][2]-v[i][2]};
        if(normalize(a,3)&&normalize(b,3))
        {
            out_productn(a,b,n);
            double temp=acos(inner_product(a,b,3));
            if(normalize(n,3)==1)
            {
                re[0]+=temp*n[0];re[1]+=temp*n[1];re[2]+=temp*n[2];
            }
        //printf("n : %lf %lf %lf %lf\n",n[0],n[1],n[2],temp);
            //SAFE_FREE(n);
        }
    }
    // return re;
}

int get_plane_equation_of_triangular_plane(
    double* tr1,double* tr2,double*tr3,double* re)
{
    double a[3]={tr2[0]-tr1[0],tr2[1]-tr1[1],tr2[2]-tr1[2]},b[3]={
        tr3[0]-tr2[0],tr3[1]-tr2[1],tr3[2]-tr2[2]};
    double n[3]={0};
    out_productn(a,b,n);
    // double * re=(double*)malloc(sizeof(double)*4);
    memset(re,0,sizeof(double)*4);
    int res=1;
    if(!normalize(n,3))
    {
        res=0;
        //free(n);
        re[0]=1.0;re[1]=0.0;re[2]=0.0;
    }
    else
    {
        re[0]=n[0];re[1]=n[1];re[2]=n[2];
    }
    re[3]=-(re[0]*tr1[0]+re[1]*tr1[1]+re[2]*tr1[2]);



    return res;
    // printf("test :%lf\n",re[0]*tr1[0]+re[1]*tr1[1]+re[2]*tr1[2] );
    // printf("test :%lf\n",re[0]*tr2[0]+re[1]*tr2[1]+re[2]*tr2[2] );
    // printf("test :%lf\n",re[0]*tr2[0]+re[1]*tr2[1]+re[2]*tr2[2] );

    //free(n);
    // return re;
}

int get_plane_equation_of_polygon(double**poly,int len,double* re)
{
    double n[3]={0};
    compute_plane_normal_of_jordan_curve( poly,len,n );
    // double * re=(double*)malloc(sizeof(double)*4);
    memset(re,0,sizeof(double)*4);
    int res=1;
    if(!normalize(n,3))
    {
        re[0]=1.0;re[1]=0;re[2]=0;
        res=0;
        // free(n);
        // return NULL;
    }
    else
    {
        re[0]=n[0];re[1]=n[1];re[2]=n[2];

    }
    re[3]=-(re[0]*poly[0][0]+re[1]*poly[0][1]+re[2]*poly[0][2]);
    // printf("test :%lf\n",re[0]*tr1[0]+re[1]*tr1[1]+re[2]*tr1[2] );
    // printf("test :%lf\n",re[0]*tr2[0]+re[1]*tr2[1]+re[2]*tr2[2] );
    // printf("test :%lf\n",re[0]*tr2[0]+re[1]*tr2[1]+re[2]*tr2[2] );

    return res;
}

//@ p1 is a plane equation
//@ p2 is a plane equation
//return t* n+ p:: line equation
int get_line_equation_from_2planes(double * p1,double *p2,double* re)
{
    double n[3]={0};
    out_productn(p1,p2,n);
    //printf("n:%.9lf %.9lf %.9lf\n",n[0],n[1],n[2]);
    // printf("get line :%lf %lf %lf\n",fabs(n[0]),fabs(n[1]),fabs(n[2]));
    // printf("p1 :%lf %lf %lf\n",p1[0],p1[1],p1[2]);
    // printf("p2 :%lf %lf %lf\n",p2[0],p2[1],p2[2]);
    // double * re=(double*)malloc(sizeof(double)*6);
    memset(re,0,sizeof(double)*6);
    // int res=1;
    if(!normalize(n,3))
    {
        //printf("not normalize\n");
        //free(n);
        re[0]=1.0;re[1]=0;re[2]=0;

        double fac=inner_product(re,p1,3);
        re[0]-=fac*p1[0];re[1]-=fac*p1[1];re[2]-=fac*p1[2];
        if(!normalize(re, 3)){
            re[0]=0;re[1]=1.0;re[2]=0;
        }

        re[3]-=(p1[3]*p1[0]);re[4]-=(p1[3]*p1[1]);re[5]-=(p1[3]*p1[2]);
        re[3]-=(p2[3]*p2[0]);re[4]-=(p2[3]*p2[1]);re[5]-=(p2[3]*p2[2]);
        re[3]*=0.5;re[4]*=0.5;re[5]*=0.5;

        return 0;
    }


    double fac=inner_product(p1,p2,3);
    double fac1=1.0/(fac*fac-1);
    // double mat[3][3]={
    //     {p1[0],p1[1],p1[2]},
    //     {p2[0],p2[1],p2[2]},
    //     {n[0],n[1],n[2]}
    // };
    double mat[3][3]={
        {fac1*(fac*p2[0]-p1[0]),fac1*(fac*p2[1]-p1[1]),fac1*(fac*p2[2]-p1[2])},
        {fac1*(fac*p1[0]-p2[0]),fac1*(fac*p1[1]-p2[1]),fac1*(fac*p1[2]-p2[2]) },
        {n[0],n[1],n[2]}
    };

    double b[3]={-p1[3],-p2[3],0};
    // double * re=(double*)malloc(sizeof(double)*6);
    for(int i=0;i<3;i++)
    {
        re[i+3]=0;
        for(int j=0;j<3;j++)
        {
            re[i+3]+=b[j]*mat[j][i];
        }
    }
    re[0]=n[0];re[1]=n[1];re[2]=n[2];
    return 1;
    // lb_matrix_free(mi);
    // lb_matrix_free(m);
    //free(n);
    // return re;
}



// static inline int is_line_same_dir_with_line_segment(double* l,double* v1 ,double* v2)
// {
//   //  is same dir
//     double l1[3]={v2[0]-v1[0],v2[1]-v1[1],v2[2]-v1[2]};


//     printf("l:%lf %lf %lf %lf %lf %lf\n",l[0],l[1],l[2],l[3] );
//     printf("point dis line :%lf\n",distance_point_to_line(v1,l) );
//     normalize(l1);

//       printf("l1:%lf %lf %lf\n",l1[0],l1[1],l1[2] );
//     double * n3=out_product(l,l1);
//     if(!normalize(n3))
//     {
//        // printf("is sanem dir\n");
//         SAFE_FREE(n3);
//         return 1;
//     }
//     SAFE_FREE(n3);
//    return 0;
// }

int line_point_nearest_2_line(double* l,double* l1,double* t1,double *t2)
{
    double b[3]={l[3]-l1[3],l[4]-l1[4],l[5]-l1[5]};

    double A[3][2]={
        {l[0],-l1[0]},
        {l[1],-l1[1]},
        {l[2],-l1[2]}
    };
    double B[2][2] = {0};

// 计算 A^T * A
    for (int i = 0; i < 2; ++i) {
        for (int j = 0; j <= i; ++j)
        {
            for (int k = 0; k < 3; ++k)
            {
                B[i][j] += A[k][i] * A[k][j];
            }
            B[j][i] = B[i][j];
        }
    }
    double det=B[0][0]*B[1][1]-B[0][1]*B[1][0];
    if(double_equal_zero(det))
    {
        *t1=0;*t2=-(A[0][0]*b[0]+A[1][0]*b[1]+A[2][0]*b[2])/B[0][1];
        return 0;
    }
    double B_inverse[2][2]={
        {B[1][1],-B[0][1]},
        {-B[1][0],B[0][0]}
    };
    double Ab[2]={A[0][0]*b[0]+A[1][0]*b[1]+A[2][0]*b[2],
        A[0][1]*b[0]+A[1][1]*b[1]+A[2][1]*b[2]
    };

    *t1=-(Ab[0]*B_inverse[0][0]+Ab[1]*B_inverse[0][1])/det;
    *t2=-(Ab[0]*B_inverse[1][0]+Ab[1]*B_inverse[1][1])/det;
    return 1;
}
//is line intersecting with line segment

// int is_line_intersecting_with_line_segment(double* l,double* v1,double *v2,double *t)
// {

//     double d1[3]={v1[0]-l[3],v1[1]-l[4],v1[2]-l[5]};
//     double d2[3]={v2[0]-l[3],v2[1]-l[4],v2[2]-l[5]};
//     double n1[3]={0},n2[3]={0};
//     out_productn(l,d1,n1);
//     out_productn(l,d2,n2);
//     int re=0;
//     //printf("lin wi:%lf %lf %lf\n",n1[0],n1[1],n1[2]);
//     //printf("lin wi:%lf %lf %lf\n",n2[0],n2[1],n2[2]);

//     if(!normalize(n1,3))
//     {re++;}
//     if(!normalize(n2,3))
//     {re++;}
//     //printf("re:%d\n",re);
//     if(re==0&&inner_product(n1,n2,3)<1e-9)
//     {
//         re=1;
//     }
//     if(re==2&&t!=NULL)
//     {
//         *t=inner_product(l,d1,3);
//     }
//     if(re==1&&t!=NULL)
//     {
//         double t1=inner_product(l,d1,3);
//         double t2=inner_product(l,d2,3);
//         double lambda=0;
//         double t1_l=SAFE_SQRT(inner_product(d1,d1,3)-t1*t1);
//         double t2_l=SAFE_SQRT(inner_product(d2,d2,3)-t2*t2);
//         if(inner_product(n1,n2,3)>0)
//         {
//             lambda=(t1_l<t2_l? 1:0);
//         }
//         else
//         {
//             lambda=t2_l/(t2_l+t1_l);
//         }
//         *t=lambda*t1+(1-lambda)*t2;
//     }
//     //free(n1);free(n2);
//     return re;
// }

// int is_point_in_line_segment(double* p,double *v1,double* v2)
// {
//     double dir1[3]={p[0]-v1[0],p[1]-v1[1],p[2]-v1[2]};
//     double dir2[3]={p[0]-v2[0],p[1]-v2[1],p[2]-v2[2]};
//     // if(!normalize(dir1,3)||!normalize(dir1,3))
//     // {
//     //     return 1;
//     // }
//     double n[3]={0};
//     out_productn(dir1,dir2,n);
//     if(fabs(n[0])<1e-10&&fabs(n[1])<1e-10&& fabs(n[2])<1e-10&& inner_product(dir1,dir2,3)<0)
//     {
//         //SAFE_FREE(n);
//         return 1;
//     }
//     //SAFE_FREE(n);
//     return 0;
// }


// int is_line_segment_intersecting_with_line_segment(double * v1,double* v2,double *vv1,double*vv2,
//     double* p)
// {
//     int re=0;
//     double l1[6]={0};
//     get_line_equation_from_two_points(v1,v2,l1);
//     double t=0;

//     if(l1!=NULL)
//     {
//         re=is_line_intersecting_with_line_segment(l1,vv1,vv2,&t);
//         //printf("is line :%d\n",re);

//         if(re==1)
//         {
//             double temp_p[3]={l1[0]*t+l1[3],l1[1]*t+l1[4],p[2]=l1[2]*t+l1[5]};
//             if(p!=NULL)
//             {
//                 p[0]=temp_p[0];p[1]=temp_p[1];p[2]=temp_p[2];
//             }

//             re=is_point_in_line_segment(temp_p,v1,v2);

//         }
//         else if(re==2)
//         {
//             double p1[3]={vv1[0]-v1[0],vv1[1]-v1[1],vv1[2]-v1[2]};
//             double p2[3]={vv2[0]-v1[0],vv2[1]-v1[1],vv2[2]-v1[2]};
//             double p3[3]={vv2[0]-v2[0],vv2[1]-v2[1],vv2[2]-v2[2]};
//             double p4[3]={vv1[0]-v2[0],vv1[1]-v2[1],vv1[2]-v2[2]};
//             if(inner_product(p1,p2,3)>0&&inner_product(p2,p3,3)>0&&
//                 inner_product(p3,p4,3)>0)
//             {
//                 re=0;
//             }
//         }
//         // SAFE_FREE(l1);
//         return re;
//     }
//     else
//     {
//         if(p!=NULL)
//         {
//             p[0]=v1[0];p[1]=v1[1];p[2]=v1[2];
//         }
//         return is_point_in_line_segment(v1,vv1,vv2);
//     }
//     return re;
// }

//
// @l :: t*n+p
// @poly::polygon is convex
// 求的是多边形边界和直线的交点，两个最近交点，排名最靠前的交点
// @return result[2][4]: id ,lambda,t,distance
//
void line_intersection_with_polygon(double* l,double** poly ,int len,
    double result[][4])
{
    memset(result[0],0,sizeof(double)*4);
    memset(result[1],0,sizeof(double)*4);
    result[0][0]=-1;
    result[1][0]=-1;

    int num=0;
    for(int i=0;i<len;i++)
    {
        double temp[4]={0};
        temp[0]=i;

        line_segment_nearest_2_line(l,poly[i],poly[(i+1)%len],
            &(temp[1]),&(temp[2]));

        double p1[3]={temp[1]*poly[i][0]+(1-temp[1])*poly[(i+1)%len][0],
            temp[1]*poly[i][1]+(1-temp[1])*poly[(i+1)%len][1],
            temp[1]*poly[i][2]+(1-temp[1])*poly[(i+1)%len][2]
        };

        double p2[3]={temp[2]*l[0]+l[3],
            temp[2]*l[1]+l[4],
            temp[2]*l[2]+l[5]
        };
        temp[3]=distance_of_two_points(p1,p2,3);
        if (num == 0) {
            result[0][0] = temp[0];
            result[0][1] = temp[1];
            result[0][2] = temp[2];
            result[0][3] = temp[3];
            num += 1;
            continue;
        }
        int flag = 0;
        for (int j=0;j<num;j++)  {
            // if (lbmath.approxEqAbs(T, temp[3] - result[j][3], 0, null))
            if(double_equal_zero(temp[3]-result[j][3]))
            {
                flag = 1;
                break;
            }
        }
        if (!flag) {
            int flag1 = 0;
            for (int j=0;j<num;j++)  {
                if (temp[3] < result[j][3]) {
                    flag1 = 1;
                    int k = 1;
                    while (k > j)  {
                        result[k][0] = result[k - 1][0];
                        result[k][1] = result[k - 1][1];
                        result[k][2] = result[k - 1][2];
                        result[k][3] = result[k - 1][3];
                        k-=1;
                    }
                    result[j][0] = temp[0];
                    result[j][1] = temp[1];
                    result[j][2] = temp[2];
                    result[j][3] = temp[3];
                    if (num < 2) {
                        num += 1;
                    }
                    break;
                }
            }
            if (flag1 == 0 && num < 2) {
                result[num][0] = temp[0];
                result[num][1] = temp[1];
                result[num][2] = temp[2];
                result[num][3] = temp[3];
                num += 1;
            }
        }
    }
}

// static inline double * get_intersection_points_from_two_line_segments(double* l11,double* l12,double* l21,double* l22 )
// {
//     double *l1=get_line_equation_from_two_p(l11,l12);
//     double* l2=get_line_equation_from_two_p(l21,l22);
//     double* re=NULL;
//     if(l1==NULL||l2==NULL)
//     {
//         SAFE_FREE(l1);SAFE_FREE(l2);
//         return NULL;
//     }

//     if(is_line_intersecting_with_line_segment(l1,l21,l22)&&is_line_intersecting_with_line_segment(l2,l11,l12))
//     {
//         double t=line_point_nearest_2_line(l1,l2);
//         re=(double*)malloc(sizeof(double)*3);
//         re[0]=t*l1[0]+l1[3];re[1]=t*l1[1]+l1[4];re[2]=t*l1[2]+l1[5];
//         return re;
//     }

//     SAFE_FREE(l1);SAFE_FREE(l2);
//     return NULL;
// }
//在同一个平面上的两个多边形相交点
// static inline double** get_intersection_points_from_two_polygons_on_one_plane(double **poly1,int len1,double** poly2,int len2)
// {
//     double ** re=(double**)malloc(sizeof(double*)*2);
//     re[0]=NULL;re[1]=NULL;
//     int temp_i=0;
//     for(int i=0;i<len1;i++)
//     {
//         int i1=i,i2=(i+1)%len1;
//         for(int j=0;j<len2;j++)
//         {
//             int j1=j,j2=(j+1)%len2;
//             double *vv=get_intersection_points_from_two_line_segments(poly1[i1],poly1[i2],poly2[j1],poly2[j2]);

//             if(vv!=NULL&&temp_i<2)
//             {
//                 re[temp_i]=vv;
//                 temp_i++;
//             }
//         }
//     }
//     return re;
// }

// 右手，法向n穿过右手掌心，大拇指方向即为多边形要提取的方向
/**/
void get_half_section_polygon_and_line3d(double** poly,int len,
    double*l,double** re,int* len1)
{
    *len1=0;
    double n[3]={0};
    compute_plane_normal_of_jordan_curve( poly,len,n );
    if(!normalize(n,3)){return;}
    double y_dir[3]={0};

    out_productn(n,l,y_dir);

    double mat[3][3]={
        {l[0],l[1],l[2]},
        {y_dir[0],y_dir[1],y_dir[2]},
        {n[0],n[1],n[2]}
    };
    // double mat1[3][3]={
    //     {mat}，
    //     {y_dir[0],y_dir[1],y_dir[2]}，
    //     {n[0],n[1],n[2]}
    // };
    double **temp_poly=(double**)malloc(sizeof(double*)*len);
    for(int i=0;i<len;i++)
    {
        double temp_p[3]={poly[i][0]-l[3],poly[i][1]-l[4],poly[i][2]-l[5]};
        temp_poly[i]=(double*)malloc(sizeof(double)*3);
        temp_poly[i][0]=mat[0][0]*temp_p[0]+mat[0][1]*temp_p[1]+mat[0][2]*temp_p[2];
        temp_poly[i][1]=mat[1][0]*temp_p[0]+mat[1][1]*temp_p[1]+mat[1][2]*temp_p[2];
        temp_poly[i][2]=mat[2][0]*temp_p[0]+mat[2][1]*temp_p[1]+mat[2][2]*temp_p[2];
    }


    int * ranse1=(int*)malloc(sizeof(int)*(len));
    memset(ranse1,0,sizeof(int)*(len));


    int ii=-1;

    for(int i=0;i<len;i++)
    {
        if(temp_poly[i][1]>1e-11)
        {
            ranse1[i]=1;
            ii=i;
        }
        else if(temp_poly[i][1]< -1e-11)
        {
            ranse1[i]=-1;
        }
    }

    if(ii!=-1)
    {
        *len1=0;
        // re=(double**)malloc(sizeof(double*)*(len+len));
        // memset(re,0,sizeof(double*)*(len+len));
        int flag=1;
        for(int i=0;i<len;i++)
        {
            // printf("ranse:%d\n",ranse1[(i+ii)%len]);
            if(ranse1[(i+ii)%len]>0||
                (ranse1[(i+ii)%len]==0&&(ranse1[(i+ii-1+len)%len]!=0
                    ||ranse1[(i+ii+1+len)%len]!=0)) )
            {
                // (*re)[*len1] =(double*)malloc(sizeof(double)*3);
                (re)[*len1][0]=poly[(i+ii)%len][0];(re)[*len1][1]=poly[(i+ii)%len ][1];(re)[*len1][2]=poly[(i+ii)%len ][1];
                *len1=*len1+1;
            }
            if(ranse1[(i+ii)%len]>0){flag=1;}else if(ranse1[(i+ii)%len]<0){flag=-1;}

            if(( ranse1[(i+ii)%len]>0&& ranse1[(i+ii+1)%len]<0)||
                ( ranse1[(i+ii)%len]<0&& ranse1[(i+ii+1)%len]>0) )
            {
                double lambda=temp_poly[(i+ii+1)%len][1]/(temp_poly[(i+ii+1)%len][1]-temp_poly[(i+ii)%len][1]);
                double p[3]={0};
                p[0]=lambda*temp_poly[(i+ii)%len][0]+(1-lambda)*temp_poly[(i+ii+1)%len][0];
                p[1]=lambda*temp_poly[(i+ii)%len][1]+(1-lambda)*temp_poly[(i+ii+1)%len][1];

                double pp[3]={0};
                pp[0]=mat[0][0]*p[0]+mat[1][0]*p[1];
                pp[1]=mat[0][1]*p[0]+mat[1][1]*p[1];
                pp[2]=mat[0][2]*p[0]+mat[1][2]*p[1];

                // pp[0]=data1[0]*p[0]+data1[1]*p[1];
                // pp[1]=data1[2]*p[0]+data1[3]*p[1];
                pp[0]+=l[4];pp[1]+=l[4];pp[2]+=l[5];

                // re[*len1] =(double*)malloc(sizeof(double)*3);
                (re)[*len1][0]=pp[0];(re)[*len1][1]=pp[1];(re)[*len1][2]=pp[2];
                *len1=*len1+1;
            }
        }
    }


    free(ranse1);
    for(int i=0;i<len;i++)
    {
        free(temp_poly[i]);
    }
    free(temp_poly);

    // free(n);
}


// void get_intersections_from_two_polygons(double**poly1,int len1,
//     double**poly2 ,int len2,double***result,int* len)
// {
//     *len=0;



// }




static int get_coordinate_in_another_coordinate_system(
    double* p,double* a1,double *a2,double*a3,double* re)
{
    LB_Matrix* m=(LB_Matrix*)malloc(sizeof(LB_Matrix));
    lb_matrix_init_double(m,3,3);
    double* data=(double*)(m->data);
    data[0*3+0]=a1[0]; data[1*3+0]=a1[1];  data[2*3+0]=a1[2];
    data[0*3+1]=a2[0]; data[1*3+1]=a2[1];  data[2*3+1]=a2[2];
    data[0*3+2]=a3[0]; data[1*3+2]=a3[1];  data[2*3+2]=a3[2];
    LB_Matrix* mi=m->inverse(m);
    if(mi==NULL)
    {
        lb_matrix_free(m);
        return 0;
    }
    data=(double*)(mi->data);
    // double * re=(double*)malloc(sizeof(double)*3);
    // memset(re,0,sizeof(double)*3);
    for(int i=0;i<3;i++)
    {
        re[i]=0;
        for(int j=0;j<3;j++)
        {
            re[i]+=data[i*3+j]*p[j];
        }
    }
    lb_matrix_free(mi);
    lb_matrix_free(m);
    return 1;
}


//return the triangle coordinate of p in p1 p2 p3
int  get_triangle_coordinate(double* p,double * p1,double*p2,double* p3 ,
    double *re)
{
    double a1[3]={p1[0]-p3[0],p1[1]-p3[1],p1[2]-p3[2]};
    double a2[3]={p2[0]-p3[0],p2[1]-p3[1],p2[2]-p3[2]};
    double pp[3]={p[0]-p3[0],p[1]-p3[1],p[2]-p3[2]};
    double n[3]={0};
    out_productn(a1,a2,n);
    double coord[3]={0};
    //free(n);
    if(get_coordinate_in_another_coordinate_system(pp,a1,a2,n,coord)==0)
    {
        return 0;
    }
    // double *re=(double*)malloc(sizeof(double)*3);
    re[0]=coord[0];re[1]=coord[1];
    re[2]=1-re[0]-re[1];
    // free(coord);

    return 1;
}

double compute_area_of_triangle(double* p1,double* p2,double* p3)
{
    double q1[3]={p2[0]-p1[0],p2[1]-p1[1],p2[2]-p1[2]};

    double q2[3]={p3[0]-p1[0],p3[1]-p1[1],p3[2]-p1[2]};

    double q[3]={q1[1]*q2[2]-q1[2]*q2[1],-q1[0]*q2[2]+q1[2]*q2[0],q1[0]*q2[1]-q1[1]*q2[0]};
    double re=SAFE_SQRT(q[0]*q[0]+q[1]*q[1]+q[2]*q[2])/2.0;
    return re;
}


void  get_half_section_polygon_and_plane3d(double** poly,int len,
    double*p,double** re,int *len1)
{
    double p1[4]={0};
    get_plane_equation_of_polygon(poly,len,p1);
    double n[3]={0};
    out_productn(p,p1,n);
    if( !normalize(n,3) )
    {
        *len1=0;
        if(p[0]*poly[0][0]+p[1]*poly[0][1]+p[2]*poly[0][2]+p[3]>=0)
        {
            *len1=len;
            for(int i=0;i<len;i++)
            {
                memmove((re)[i],poly[i],sizeof(double)*3);
            }
        }
        return;
    }

    double l1[6]={0};
    get_line_equation_from_2planes(p,p1,l1);

    // if(l1==NULL){free(p1);return ;}

    get_half_section_polygon_and_line3d(poly,len,
        l1,re,len1);

    // free(p1);
    // free(l1);

    // return re;
}

//
//不管 poly1 poly2 是不是在一个平面，返回的是相交区域，可以是null,点，线，面
// 保证poly1和poly2是凸的
//
void get_intersections_from_two_polygons(double**poly1,int len1,
    double**poly2 ,int len2,double**result,int* len)
{
    *len=0;
    double p1[4]={0};
    get_plane_equation_of_polygon(poly1,len1,p1);

    double p2[4]={0};
    get_plane_equation_of_polygon(poly2,len2,p2);
    double n[3]={0};
    out_productn(p1,p2,n);
    if( !normalize(n,3) )
    {
        if(!double_equal_zero(inner_product(p1,poly2[0],3)+p1[3]) )
        {
            return;
        }
        int step=0;
        inner_product(p1,p2,3)>0?(step=1):(step=-1);
        double **temp_poly1=(double**)malloc(sizeof(double*)*(len1+len2));
        double **temp_poly2=(double**)malloc(sizeof(double*)*(len1+len2));
        for(int j=0;j<len1+len2;j++)
        {
            temp_poly1[j]=(double*)malloc(sizeof(double)*3);
            temp_poly2[j]=(double*)malloc(sizeof(double)*3);
        }
        int temp_len1=0,temp_len2=0;
        for(int j=0;j<len2;j++)
        {
            memmove(temp_poly1[j],poly2[j],sizeof(double)*3);
        }
        temp_len1=len2;
        double l[6]={0};
        int i=0;
        do
        {
            int j=(i+step+len1)%len1;
            get_line_equation_from_two_points(poly1[i],poly1[j],l);

            get_half_section_polygon_and_line3d(temp_poly1,temp_len1,
                l,temp_poly2,&temp_len2);

            temp_len1=temp_len2;
            double** temp_poly3=temp_poly1;
            temp_poly1=temp_poly2;temp_poly2=temp_poly3;
            if(temp_len1==0){break;}
            i=j;
        }while(i!=0);
        *len=temp_len1;
        for(int j=0;j<temp_len1;j++)
        {
            memmove(result[j],temp_poly1[j],sizeof(double)*3);
        }

        for(int j=0;j<len1+len2;j++)
        {
            free(temp_poly1[j]);
            free(temp_poly2[j]);
        }
        free(temp_poly1);free(temp_poly2);
        return;
    }

    *len=0;
    double l[6]={0};
    get_line_equation_from_2planes(p1,p2,l);

    double temp_re1[2][4]={0};
    line_intersection_with_polygon(l,poly1 ,len1,
        temp_re1);

    if(temp_re1[0][0]==-1){return;}

    double min_max1[2]={temp_re1[0][2],temp_re1[0][2]};
    if(temp_re1[1][0]!=-1)
    {
        if(temp_re1[1][2]<min_max1[0])
        {
            min_max1[0]=temp_re1[1][2];
        }
        else if(temp_re1[1][2]>min_max1[1])
        {
            min_max1[1]=temp_re1[1][2];
        }
    }

    double temp_re2[2][4]={0};

    line_intersection_with_polygon(l,poly2 ,len2,
        temp_re2);
    if(temp_re2[0][0]==-1){return;}

    double min_max2[2]={temp_re2[0][2],temp_re2[0][2]};
    if(temp_re2[1][0]!=-1)
    {
        if(temp_re2[1][2]<min_max2[0])
        {
            min_max2[0]=temp_re2[1][2];
        }
        else if(temp_re2[1][2]>min_max2[1])
        {
            min_max2[1]=temp_re2[1][2];
        }
    }


    if(min_max1[0]<=min_max2[1]&&min_max1[1]>=min_max2[0])
    {
        double min_max[2]={0};
        min_max1[0]<min_max2[0]?(min_max[0]=min_max2[0]):(min_max[0]=min_max1[0]);
        min_max1[1]<min_max2[1]?(min_max[1]=min_max1[1]):(min_max[1]=min_max2[1]);
        *len=1;
        result[0][0]=min_max[0]*l[0]+l[3];
        result[0][1]=min_max[0]*l[1]+l[4];
        result[0][2]=min_max[0]*l[2]+l[5];
        if(double_equal_zero(min_max[0]-min_max[1])==0)
        {
            *len=2;
            result[1][0]=min_max[1]*l[0]+l[3];
            result[1][1]=min_max[1]*l[1]+l[4];
            result[1][2]=min_max[1]*l[2]+l[5];

        }
    }
}




// double** get_intersection_polygon_and_polygon(double** poly,int len,double** poly1,int len1,
//     int *len2,int*tuihua)
// {
//     int temp_len=len1;
//     double** re=(double**)malloc(sizeof(double*)*temp_len);
//     for(int i=0;i<temp_len;i++)
//     {
//         re[i]=(double*)malloc(sizeof(double)*3);
//         re[i][0]=poly1[i][0];re[i][1]=poly1[i][1];re[i][2]=0;
//     }

//     for(int i=0;i<len;i++)
//     {
//         double l[4]={poly[(i+1)%len][0]-poly[i][0],poly[(i+1)%len][1]-poly[i][1],poly[i][0],poly[i][1]};
//         //double *l= get_line_equation_from_two_points(poly[i],poly[(i+1)%len]);
//         //printf("here i:%d %d\n",i,temp_len );
//         if(normalize(l,2))
//         {
//             //printf("%lf %lf\n",l[0],l[1]);
//             int temp_len1=0;
//             double**temp_re= get_half_section_polygon_and_line(re,temp_len,&temp_len1 ,l,tuihua);
//             //printf("here %d\n",temp_len1);
//             for(int j=0;j<temp_len;j++)
//             {
//                 free(re[j]);
//             }
//             free(re);
//             re=temp_re;temp_len=temp_len1;
//             if(re==NULL){break;}
//         }
//     }
//     *len2=temp_len;

//     return re;
// }
double * get_one_dir_trans2dir_rota_matrix(double* n1,double*n2)
{
    double n[3]={0};
    out_productn(n1,n2,n);
    if(!normalize(n,3))
    {
        //SAFE_FREE(n);

        double temp_dir[3]={1,0,0};
        out_productn(n1,temp_dir,n);
        if(!normalize(n,3))
        {
            //SAFE_FREE(n);
            temp_dir[1]=1;
            out_productn(n1,temp_dir,n);
            if(!normalize(n,3))
            {
                //SAFE_FREE(n);
                return NULL;
            }
        }
    }
    if(!normalize(n1,3)||!normalize(n2,3))
    {
        //SAFE_FREE(n);
        return NULL;
    }
    double theta=acos(inner_product(n1,n2,3));
    double * re= get_matrix_from_rotation(theta,n);

    //SAFE_FREE(n);
    return re;
}

static double * get_neareast_point2polygon1(double** poly,int len,double* p,double* n)
{
    double*re=(double*)malloc(sizeof(double)*3);
    re[0]=p[0];re[1]=p[1];re[2]=p[2];
    double* angles=(double*)malloc(sizeof(double)*len);
    //memset(angles,0,sizeof(double)*len);
    for(int i=0;i<len;i++)
    {
        angles[i]=-10;
    }
    int ii=-1;
    double n1[3]={0};
    for(int i=0;i<len;i++)
    {
        double dir[3]={poly[i][0]-poly[(i+1)%len][0],poly[i][1]-poly[(i+1)%len][1],poly[i][2]-poly[(i+1)%len][2]};
        double dir1[3]={p[0]-poly[i][0],p[1]-poly[i][1],p[2]-poly[i][2]};
        if(!normalize(dir,3))
        { continue;}
        if(!normalize(dir1,3))
        {
            free(angles);
            return re;
        }

        angles[i]=acos(-inner_product(dir,dir1,3));
        out_productn(dir,dir1,n1);
        if(inner_product(n,n1,3)<0)
        {
            angles[i]=-angles[i];
        }

        //free(n1);
        if(angles[i]>=0)
        {
            ii=i;break;
        }
    }

    if(ii!=-1)
    {
        int step=1;
        if(ii==0&&angles[ii]>M_PI*0.5 )
        {step=-1;}

        int jj=(ii+step+len )%len;
        while(angles[jj]==-10)
        {
            double dir[3]={poly[jj][0]-poly[(jj+1)%len][0],poly[jj][1]-poly[(jj+1)%len][1],poly[jj][2]-poly[(jj+1)%len][2]};
            double dir1[3]={p[0]-poly[jj][0],p[1]-poly[jj][1],p[2]-poly[jj][2]};
            if(!normalize(dir,3))
            {
                //ii=jj;
                jj=(jj+step+len )%len;
                continue;
            }
            if(!normalize(dir1,3))
            {
                free(angles);
                return re;
            }
            angles[jj]=acos(-inner_product(dir,dir1,3));
            out_productn(dir,dir1,n1);
            if(inner_product(n,n1,3)<0)
            {
                angles[jj]=-angles[jj];
            }

            //free(n1);

            if(step==1&& !(angles[jj]<M_PI*0.5&&angles[jj]>0) )
            {
                break;
            }
            if(step==-1&& angles[jj]<=M_PI*0.5)
            {
                break;
            }
            ii=jj;
            jj=(jj+step+len )%len;
        }
        double p1[3]={poly[jj][0]-p[0],poly[jj][1]-p[1],poly[jj][2]-p[2]  };
        double p2[3]={poly[ii][0]-poly[jj][0],poly[ii][1]-poly[jj][1],poly[ii][2]-poly[jj][2]  };
        double lambda=-(inner_product(p1,p2,3))/(p2[0]+p2[1]+p2[2]);
        //
        //此时lambda可能超过线段范围，也就是不在0-1范围
        //
        re[0]=lambda*p2[0]+poly[jj][0];re[1]=lambda*p2[1]+poly[jj][1];re[2]=lambda*p2[2]+poly[jj][2];

    }
    free(angles);
    return re;
}

double * get_neareast_point2polygon2d(double** poly,int len,double* p)
{
    double n[3]={0};
    compute_plane_normal_of_jordan_curve( poly,len,n );
    normalize(n,3);
    double *re= get_neareast_point2polygon1(poly,len, p, n);

    // free(n);


    return re;
}
double * get_neareast_point2polygon3d(double** poly,int len,double* p)
{
    double n[3]={0};
    compute_plane_normal_of_jordan_curve( poly,len,n );
    normalize(n,3);

    double p1[3]={poly[0][0]-p[0],poly[0][1]-p[1],poly[0][2]-p[2] };

    double la=inner_product(n,p1,3);
    double p2[3]={p[0]+la*n[0],p[1]+la*n[1],p[2]+la*n[2]};

    double *re= get_neareast_point2polygon1(poly,len, p2, n);

    // free(n);
    return re;
}
